// 1) Include Resources, define Variables and functions

#include <p24FJ64GA004.h>
#include "GenericTypeDefs.h"

#define portbit_set(var,bitno) {var##Write |= 1 << bitno; var=var##Write;}
#define portbit_clr(var,bitno) {var##Write &= ~(1 << bitno);var=var##Write;}
#define bit_set(var,bitno) var |= 1 << bitno
#define bit_clr(var,bitno) var &= ~(1 << bitno)
#define bit_assign(var,bitno,bitval) {var |= bitval << bitno; var &= ~(~ (bitval) << bitno);}
#define CS_clr portbit_clr(PORTB,9)
#define CS_set portbit_set(PORTB,9)
#define get_bit(data,bitno) ((data>>bitno)&0x01)
#define confPWM(n) {PR##n = 0x7fff; OC##n##RS = 0x3333; OC##n##R = OC##n##RS; OC##n##CON = 0x000E; bit_set(T##n##CON, 15);}

#define _ISRFASTPSV __attribute__((interrupt, shadow, auto_psv))

#define delay_1ms {int j;for(j=0;j < 3000;j++){}}
#define delay_10us {int j;for(j=0;j < 30;j++){}}

static unsigned int PORTCWrite=0;
static unsigned int PORTBWrite=0;
static unsigned int PORTAWrite=0;
static int Received_Packet[42];

unsigned char GetShortRAMAddress(unsigned char address);
unsigned char GetLongRAMAddress(int address);
void SetShortRAMAddress(unsigned char address, unsigned char value);
void SetLongRAMAddress(int address, unsigned char value);

int Ball_Possessed; //Stores the ball possession state
int Motor_Speed_Out[4]; //Stores the motor speed
int Motor_Direction[4]; //Stores the motor direction
int Motor_Speed_In[4]; //Store the current motor speed from Minitors
int Charge_Time; //Stores the desired charge time, which determines the kick strength
int Dribbler_Speed; //Stores the desired dribbler speed
int Motor_Braked[4]; //Stores whether the motors are breaked
int Called_Robot_ID;

_CONFIG1(GCP_OFF & GWRP_OFF & FWDTEN_OFF);
_CONFIG2( FNOSC_PRI &  POSCMOD_HS); //primary oscilator enabled, high speed oscillator

int main()
{
// 2) Configure the Microcontroller  



  bit_set(INTCON2, 1); //interrupt INT1 on negative edge
  bit_clr(IFS1, 4); //External Interrupt flag status: interrupt has not occurred
  bit_set(IEC1, 4); // Interrupt request enabled for INT1

INTCON2 = 0x01; //INT0EP: External Interrupt 0 Edge Detect Polarity Select bit : Interrupt on negative edge 

//Set up the Pins I/O sources for the RP (Remapable Peripheral) Pins
//Unlock the RPINRx and RPORx registers to write to them, the unlock sequence should be performed by writing inline assembly.
// Unlock Registers
//*************************************
asm volatile ( "MOV #OSCCON, w1 \n"
 "MOV #0x46, w2 \n"
 "MOV #0x57, w3 \n"
 "MOV.b w2, [w1] \n"
 "MOV.b w3, [w1] \n"
 "BCLR OSCCON,#6");	

RPOR3  =  0x0012; //connect RP6 to OC1 (18)
RPOR10 =  0x0013; //connect RP20 to OC2 (19)
RPOR2 =   0x0014; //set to 20: RP4 <= OC3 (20)
RPOR7 =   0x1500; //connect RP15/RB15 to OC4 (21)

RPINR0 =  0x0A00; //set External interrupt 1 to RP10

asm volatile ( "MOV #OSCCON, w1 \n"
 "MOV #0x46, w2 \n"
 "MOV #0x57, w3 \n"
 "MOV.b w2, [w1] \n"
 "MOV.b w3, [w1] \n"
 "BSET OSCCON, #6" );

  IPC5 = 0x0007;   //  Set the priority level for INT1 to 7
  
  /* ########port A:
     0: brake4 (output)
     1: dir4 (output)
     2: xtal
     3: xtal
     4: dir3 (output)
     5
     6
     7: dribbler onoff (output)
     8: brake3 (output)
     9: brake2 (output)
     10: ironoff (output)
end Port A */
  TRISA = 0b1111100001101100;

   /*########port B:
     0: debugger
     1: debugger
     2: done (input)
     3: charge (output)
     4: speed3 (output)
     5: monitor1 (input)
     6: speed1 (output)
     7: brake1 (output)
     8: dir1 (output)
     9: CS radio (output)
     10: INT1 radio (input)
     11: WAKE radio (output)
     12: reset radio (output)
     13: irdetect (input)
     14: monitor4 (input)
     15: speed4 (output)
	end Port B*/
  TRISB = 0b0110010000100111;   

	/*########port C:
     0: kick (output)
     1: -
     2: monitor3 (input)
     3: monitor2 (input)
     4: speed2 (output)
     5: dir2 (output)
     6: SDI2 (input)
     7: SCK2 (output)
     8: SDO2 (output)
     9: -
  end Port C*/

  TRISC = 0b1111111001001110;
  

  //Set up the SPI interface
  //To set up the SPI module for the Standard Master mode
  //of operation:
  //1. If using interrupts:
  //a) Clear the SPIxIF bit in the respective IFSx
  //register.
  bit_clr(IFS2, 1);  //SPI2 Event Interrupt Flag Status bit: Interrupt request has not occurred
  bit_clr(IFS2,0); //SPI2 Fault Interrupt Flag Status bit: Interrupt request has not occurred
  //bit_set(IEC2, 1);           //b) Set the SPIxIE bit in the respective IECx register: SPI2 Event Interrupt Request enabled
  //bit_set(IEC2, 0);
  IPC8 = 0b0000000001100111;	//c) Write the SPIxIP bits in the respective IPCx register to set the interrupt priority:
  // SPI2 Fault Interrupt priority = 7 and Event Interrupt priority = 6
  SPI2STAT = 0x8000; //4. Enable SPI operation by setting the SPIEN bit (SPIxSTAT<15>): MUST occur after setting SPI2CON registers

AD1PCFG = 0xffff;  //selects AN0-12 as analog inputs
  PORTCWrite=0;
  PORTBWrite=0;
  PORTAWrite=0;
  
  //***Initialize PWM***
  /*   PR4 = 0x7fff;  //PWM freq 488 Hz, resolution 15 bits,  */
  /*   //1. Set the PWM period by writing to the selected Timer Period register (PRy). */
  /*   //PWM Period = (PR2 + 1) * TCY * (Timer 2 Prescale Value) */
  /*   OC4RS = 0x3333; //2. Set the PWM duty cycle by writing to the OCxRS register. */
  /*   OC4R = OC4RS; //3. Write the OCxR register with the initial duty cycle. */
  /*   //4. Enable interrupts, if required, for the timer and output compare modules. The output compare interrupt is required for PWM Fault pin utilization. */
  /*   // we won't use any interrupts for now, but may need to add them later if the PWM fails and we need to reset after faults!!! */
  /*   OC4CON = 0x000E; //5. Configure the output compare module for one of two PWM Operation modes by writing to the Output Compare Mode bits, OCM<2:0> (OCxCON<2:0>). */
  /*   //110 = PWM mode on OCx, Fault pin, OCFx, disabled */
  /*   bit_set(T4CON, 15); //6. Set the TMRy prescale value and enable the time base by setting TON (TxCON<15>) = 1. */
  /*   //We don't set anything because the prescale ratio is 1:1 (see Table 16-4) */

  bit_set(INTCON1, 15); //Set the NSTDIS Control bit (INTCON1<15>) : nested interrupts are not desired.

	confPWM(1);
	confPWM(2);
	confPWM(3);
	confPWM(4);

 // 3) Wait Until Packet is received 

  int i;
  while(1)
    {
      i = i + 0;
    } 
  
} //End of Main



// 4) Send kick, motor, control commands according to packet as well as IR detector

//void __attribute__((__interrupt__)) isr0(void);

//IRQ#   Primary Name     Alternate           Name   Vector     Function
//0      _INT0Interrupt   _AltINT0Interrupt   INT0   External   interrupt 0

//**interrupt service routine **
void _ISRFASTPSV _INT1Interrupt(void) //Interrupt when the first bit comes in from the radio
{
	__asm__ volatile("disi #0x3FFF"); /* disable interrupts */
    delay_1ms; //Note, this will have to be fiddled with experimentally
	int i;

	//put the packet from the radio into an array
	for (i=0; i <= 41; i++)
	{
		Received_Packet[i] = get_bit(PORTB,9);
		delay_10us;	//This will also have to be adjusted
				//Altogether there are two times that need adjusting
	}
	//store sections of the packet into subvariables
	Called_Robot_ID = 4*Received_Packet[2] + 2*Received_Packet[1] + Received_Packet[0];
	if (Received_Packet[3] == 1)	//then we are receiving a kick command
	{
		//the ball is in the path
		if (! get_bit(PORTB,13))  
		{
			Charge_Time = 8*Received_Packet[7] + 4*Received_Packet[6] + 2*Received_Packet[5] + Received_Packet[4];	
			for(i=1; i <= Charge_Time; i++) delay_10us;
			bit_set(PORTC,0);	//kick the ball
			delay_1ms; //pause while the kicker kicks
			bit_clr(PORTC,0);   //retract the kicker
		}
	}
	else
	{
		//Set the directions:
		bit_assign( PORTB, 8,  Received_Packet[8]);//DIR1
		bit_assign( PORTC, 5,  Received_Packet[9]);//DIR2
		bit_assign( PORTA, 4,  Received_Packet[10]);//DIR3
		bit_assign( PORTA, 1,  Received_Packet[11]);//DIR4
		//Set the motor speeds:		
		OC1CON = Received_Packet[12] //Register that controls Motor1 speed
				+ (10^1)*Received_Packet[13]
				+ (10^2)*Received_Packet[14]
				+ (10^3)*Received_Packet[15]
				+ (10^4)*Received_Packet[16] 
				+ (10^5)*Received_Packet[17]
				+ (10^6)*Received_Packet[18]
				+ (10^7)*Received_Packet[19]
				+ (10^8)*Received_Packet[20] 
				+ (10^9)*Received_Packet[21]
				+ (10^10)*Received_Packet[22] 
				+ (10^11)*Received_Packet[23]
				+ (10^12)*Received_Packet[24] 
				+ (10^13)*Received_Packet[25] 
				+ (10^14)*Received_Packet[26] 
				+ (10^15)*Received_Packet[27]; 
		OC2CON = //Register that controls Motor2 speed
			   Received_Packet[28] 
				+ (10^1)*Received_Packet[29] 
				+ (10^2)*Received_Packet[30] 
				+ (10^3)*Received_Packet[31] 
				+ (10^4)*Received_Packet[32] 
				+ (10^5)*Received_Packet[33] 
				+ (10^6)*Received_Packet[34] 
				+ (10^7)*Received_Packet[35] 
				+ (10^8)*Received_Packet[36] 
				+ (10^9)*Received_Packet[37] 
				+ (10^10)*Received_Packet[38] 
				+ (10^11)*Received_Packet[39] 
				+ (10^12)*Received_Packet[40] 
				+ (10^13)*Received_Packet[41] 
				+ (10^14)*Received_Packet[42] 
				+ (10^15)*Received_Packet[43]; 
		OC3CON = //Register that controls Motor3 speed
				+ (10^0)*Received_Packet[44] 
				+ (10^1)*Received_Packet[45] 
				+ (10^2)*Received_Packet[46] 
				+ (10^3)*Received_Packet[47] 
				+ (10^4)*Received_Packet[48] 
				+ (10^5)*Received_Packet[49] 
				+ (10^6)*Received_Packet[50] 
				+ (10^7)*Received_Packet[51] 
				+ (10^8)*Received_Packet[52] 
				+ (10^9)*Received_Packet[53] 
				+ (10^10)*Received_Packet[54] 
				+ (10^11)*Received_Packet[55] 
				+ (10^12)*Received_Packet[56] 
				+ (10^13)*Received_Packet[57] 
				+ (10^14)*Received_Packet[58] 
				+ (10^15)*Received_Packet[59]; 
		OC4CON = //Register that controls Motor4 speed		
				+ (10^0)*Received_Packet[60] 
				+ (10^1)*Received_Packet[61] 
				+ (10^2)*Received_Packet[62] 
				+ (10^3)*Received_Packet[63] 
				+ (10^4)*Received_Packet[64] 
				+ (10^5)*Received_Packet[65] 
				+ (10^6)*Received_Packet[66] 
				+ (10^7)*Received_Packet[67] 
				+ (10^8)*Received_Packet[68] 
				+ (10^9)*Received_Packet[69] 
				+ (10^10)*Received_Packet[70] 
				+ (10^11)*Received_Packet[71] 
				+ (10^12)*Received_Packet[72] 
				+ (10^13)*Received_Packet[73] 
				+ (10^14)*Received_Packet[74] 
				+ (10^15)*Received_Packet[75]; 
	}		
	
	    bit_clr(IFS0, 0); //turn the interrupt flag off
		DISICNT = 0x0000; //enable interrupts 
}

